#include "Force.h"

double Gravity::g = -9.8;
ActiveForce::ActiveForce(Body* Bi_ptr, const Vector3d& loc, const Vector3d& f, bool b):Unary(Bi_ptr,loc)
{
	F = f;
	adhesion = b;
}

VectorXd ActiveForce::CalForce(double t, double* y, double* dy)
{
	Vector3d Fo;
	Matrix3d Ai;
	Bi->A(Ai);
	if (adhesion)
		Fo = Ai * F;
	else
		Fo = F;
	return Fo;
}

VectorXd ActiveForce::CalTorque(double t, double* y, double* dy)
{
	Vector3d Mo;
	Matrix3d Ai;
	Bi->A(Ai);
	if (adhesion)
		Mo = rho.cross(F);
	else
		Mo = (Ai * rho).cross(F);
	return Mo;
}

inline unsigned int ActiveForce::Type() const
{
	return ForceBase::ACTIVEFORCE;
}

bool ActiveForce::Write(OUT Json::Value& force) const
{
	force["Type"] = Json::Value("ActiveForce");
	force["Bi_Id"] = Json::Value(Bi->GetID());
	for (unsigned int i = 0; i < 3; ++i)
		force["Rho"].append(Json::Value(rho(i)));
	for (unsigned int i = 0; i < 3; ++i)
		force["force"].append(Json::Value(F(i)));
	force["Adhesion"] = Json::Value(adhesion);
	return true;
}

ActiveTorque::ActiveTorque(Body* Bi_ptr, const Vector3d& m, bool b):Unary(Bi_ptr,Vector3d::Zero())
{
	M = m;
	adhesion = b;
}

VectorXd ActiveTorque::CalTorque(double t, double* y, double* dy)
{
	if (adhesion)
	{
		Matrix3d Ai;
		Bi->A(Ai);
		return Ai * M;
	}
	else
		return M;
}

inline unsigned int ActiveTorque::Type() const
{
	return ForceBase::ACTIVETORQUE;
}

bool ActiveTorque::Write(OUT Json::Value& force) const
{
	force["Type"] = Json::Value("ActiveTorque");
	force["Bi_Id"] = Json::Value(Bi->GetID());
	for (unsigned int i = 0; i < 3; ++i)
		force["Rho"].append(Json::Value(rho(i)));
	for (unsigned int i = 0; i < 3; ++i)
		force["torque"].append(Json::Value(M(i)));
	force["Adhesion"] = Json::Value(adhesion);
	return true;
}

VectorXd ForceBase::CalForce(double t, double* y, double* dy)
{
	return Vector3d::Zero();
}

VectorXd ForceBase::CalTorque(double t, double* y, double* dy)
{
	return Vector3d::Zero();
}

JointForce::JointForce(JointBase* j, const VectorXd& ft)
{
	Hj = j;
	Ft = ft;
}

VectorXd JointForce::CalForce(double t)
{
	return Ft;
}

inline unsigned int JointForce::DOF() const
{
	return Hj->DOF();
}

bool JointForce::Write(OUT Json::Value& force) const
{
	force["Type"] = Json::Value("JointForce");
	force["Bi_Id"] = Json::Value(Hj->GetBiId());
	for (int i = 0; i < Ft.rows(); ++i)
		force["Magnitude"].append(Json::Value(Ft(i)));
	return true;
}


Gravity::Gravity(Body* Bi_ptr):ActiveForce(Bi_ptr,Vector3d::Zero(),Vector3d(0,0,Bi_ptr->TotalMass()*g))
{

}

inline unsigned int Gravity::Type() const
{
	return GRAVITY;
}

bool Gravity::Write(OUT Json::Value& force) const
{
	force["Type"] = Json::Value("Gravity");
	force["Bi_Id"] = Json::Value(Bi->GetID());
	return true;
}

int Unary::Bi_Id() const
{
	return Bi->GetID();
}
